#ifndef WRENCHSTAMPED_VISUAL_H
#define WRENCHSTAMPED_VISUAL_H

#include <geometry_msgs/Wrench.h>

namespace Ogre
{
class Vector3;
class Quaternion;
}

namespace rviz
{
class Arrow;
class BillboardLine;
}

namespace rviz
{
// Each instance of WrenchStampedVisual represents the visualization of a single
// sensor_msgs::WrenchStamped message.  Currently it just shows an arrow with
// the direction and magnitude of the acceleration vector, but could
// easily be expanded to include more of the message data.
class WrenchVisual
{
public:
  // Constructor.  Creates the visual stuff and puts it into the
  // scene, but in an unconfigured state.
  WrenchVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node);

  // Destructor.  Removes the visual stuff from the scene.
  virtual ~WrenchVisual();

  // Configure the visual to show the given force and torque vectors
  void setWrench(const Ogre::Vector3& force, const Ogre::Vector3& torque);
  // Configure the visual to show the data in the message.
  void setWrench(const geometry_msgs::Wrench& wrench);

  // Set the pose of the coordinate frame the message refers to.
  // These could be done inside setMessage(), but that would require
  // calls to FrameManager and error handling inside setMessage(),
  // which doesn't seem as clean.  This way WrenchStampedVisual is only
  // responsible for visualization.
  void setFramePosition(const Ogre::Vector3& position);
  void setFrameOrientation(const Ogre::Quaternion& orientation);

  // Set the color and alpha of the visual, which are user-editable
  // parameters and therefore don't come from the WrenchStamped message.
  void setForceColor(float r, float g, float b, float a);
  void setTorqueColor(float r, float g, float b, float a);
  void setForceScale(float s);
  void setTorqueScale(float s);
  void setWidth(float w);
  void setHideSmallValues(bool h);
  void setVisible(bool visible);

private:
  // The object implementing the wrenchStamped circle
  rviz::Arrow* arrow_force_;
  rviz::Arrow* arrow_torque_;
  rviz::BillboardLine* circle_torque_;
  rviz::Arrow* circle_arrow_torque_;
  float force_scale_, torque_scale_, width_;
  bool hide_small_values_;

  // A SceneNode whose pose is set to match the coordinate frame of
  // the WrenchStamped message header.
  Ogre::SceneNode* frame_node_;
  // allow showing/hiding of force / torque arrows
  Ogre::SceneNode* force_node_;
  Ogre::SceneNode* torque_node_;

  // The SceneManager, kept here only so the destructor can ask it to
  // destroy the ``frame_node_``.
  Ogre::SceneManager* scene_manager_;
};

} // end namespace rviz

#endif // WRENCHSTAMPED_VISUAL_H
